package com.hitqz.robot.driver.zmq.protocol.handler;

import com.example.tutorial.AddressBookProtos;
import com.hitqz.robot.driver.dto.PoseDto;
import com.hitqz.robot.driver.util.WheelCacheUtil;
import com.hitqz.robot.driver.util.WheelToolkit;
import lombok.extern.slf4j.Slf4j;
import navconnector.NavconnectorBase;
import org.springframework.stereotype.Component;

/**
 * 当前位置处理类
 *
 * @author wengyx
 * @date 2021-09-14 9:45
 **/
@Slf4j
@Component
public class PoseStampedHandler implements HandlerZMQService {


    @Override
    public void handlerMsg(String robotSn, byte[] data) throws Exception {
        AddressBookProtos.PoseStamped poseStamped = AddressBookProtos.PoseStamped.parseFrom(data);
        String type = poseStamped.getDescriptorForType().getName();
        int objId = poseStamped.getHeader().getObj();
        int ackId = poseStamped.getHeader().getAck();
        NavconnectorBase.Pose pose = poseStamped.getPose();
        NavconnectorBase.Point position = pose.getPosition();
        NavconnectorBase.Euler orientation = pose.getOrientation();
        switch (objId) {
            case AddressBookProtos.PoseStamped.OBJ_ID.NUL_CURR_POSE_NTF_VALUE->{
                // 当前位置 主动上报
                PoseDto poseDto = new PoseDto();
                poseDto.setX(Double.valueOf(position.getX()).floatValue());
                poseDto.setY(Double.valueOf(position.getY()).floatValue());
                poseDto.setZ(Double.valueOf(position.getZ()).floatValue());
                poseDto.setYaw(Double.valueOf(orientation.getYaw()).floatValue());
                WheelCacheUtil.put(WheelCacheUtil.KEY_CURR_POSE, poseDto);
            }
            //里程计位置
            case AddressBookProtos.PoseStamped.OBJ_ID.NUL_ODOM_POSE_NTF_VALUE->{

            }
        }
        //释放锁
        WheelToolkit.release(ackId,type, objId, true);
    }
}
